//
// This example demonstrates how to read a series of dicom images
// and how to scroll with the mousewheel or the up/down keys
// through all slices
//
// some standard vtk headers
#include <vtkSmartPointer.h>
#include <vtkObjectFactory.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderer.h>
#include <vtkActor.h>
// headers needed for this example
#include <vtkImageViewer2.h>
#include <vtkDICOMImageReader.h>
#include <vtkInteractorStyleImage.h>
#include <vtkActor2D.h>
#include <vtkTextProperty.h>
#include <vtkTextMapper.h>
#include <vtkMarchingCubes.h>
#include <vtkPolyDataMapper.h>
#include <vtkStripper.h>
#include <vtkAxesActor.h>
#include <vtkSphereSource.h>
// needed to easily convert int to std::string
#include <sstream>
#include "vtkAutoInit.h"
#include <vtkProperty.h>
#include <vtkPolyDataNormals.h>
#include <vtkSmoothPolyDataFilter.h>
#include <vtkCamera.h>
#include <vtkSTLWriter.h>
#include <vtkCaptionActor2D.h>

#include <memath/MeVtkMath.h>

VTK_MODULE_INIT(vtkRenderingOpenGL2);
VTK_MODULE_INIT(vtkInteractionStyle);

using namespace std;

static double colorRed[3] = {1, 0, 0};
static double colorGreen[3] = {0, 1, 0};

vector<Eigen::Vector3d> sPose1PtsIron_W = {
    {197.65, 143.65, 18.23},
    {197.65, 180.42, 55},
    {197.65, 143.65, 91.77},
    {197.65, 106.88, 55},
    {197.65, 143.65, 28.23},
    // bot
    {119.65, 110.15, 85},
    {119.65, 177.15, 92},
    {119.65, 177.15, 18},
    {119.65, 110.15, 25},
    {119.65, 105.20, 89.95},
    {119.65, 122.09, 86.25},
};

vector<Eigen::Vector3d> sPose2PtsIron_W = {
    {55, 1, 18.23},
    {91.77, 1, 55},
    {55, 1, 91.77},
    {18.23, 1, 55},
    {55, 1, 28.23},
    // bot
    {21.48, 79, 85},
    {88.5, 79, 92},
    {88.5, 79, 18},
    {21.5, 79, 25},
    {16.55, 79, 89.95},
    {33.44, 79, 86.25},
};

vtkSmartPointer<vtkActor> createBallActor(double p[], double color[])
{
    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
    vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New();
    sphereSource->SetCenter(p);
    sphereSource->SetRadius(5);
    mapper->SetInputConnection(sphereSource->GetOutputPort());

    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
    actor->SetMapper(mapper);
    actor->GetProperty()->SetColor(color);

    return actor;
}

vtkSmartPointer<vtkActor> createActorWithPts(vector<Eigen::Vector3d> &pts, double color[])
{
    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
    for (uint i=0; i<pts.size(); i++) {
        double *pt = pts[i].data();
        points->InsertPoint(i, pt);
        vertices->InsertNextCell(1);
        vertices->InsertCellPoint(i);
        if (i > 0) {
            vtkIdType cellId[2] = { i-1, i };
            lines->InsertNextCell( 2, cellId );
        } else {
            vtkIdType cellId[2] = { (int)pts.size() -1, 0 };
            lines->InsertNextCell( 2, cellId );
        }
    }

    vtkSmartPointer<vtkPolyData> polygonPolyData = vtkSmartPointer<vtkPolyData>::New();
    polygonPolyData->SetPoints(points);
    polygonPolyData->SetVerts(vertices);
    polygonPolyData->SetLines(lines);

    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
    mapper->SetInputData(polygonPolyData);

    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
    actor->SetMapper(mapper);
    actor->SetScale(1);
    actor->GetProperty()->SetPointSize(4.0);
    actor->GetProperty()->SetColor(color);

    return actor;
}

/// 创建一个二维标尺和CT之间的转化矩阵
/// center 旋转中心点
/// translation 为偏移中心点
vtkSmartPointer<vtkMatrix4x4> createMatrix_CtfrW(Eigen::Vector3d &center, Eigen::Vector3d &translation,
                                            Eigen::Vector3d &anglesInDegree)
{

    Eigen::Vector3d anglesInRadian = anglesInDegree * M_PI/ 180.0;
    // 计算旋转R
    vtkSmartPointer<vtkMatrix4x4> m = MeVtkMath::rotateZYX(anglesInRadian[0], anglesInRadian[1], anglesInRadian[2]);

    // 计算t
    Eigen::Vector3d offset = MeVtkMath::multiplyPoint(m, center);
    m->Element[0][3] = translation[0]-offset[0];
    m->Element[1][3] = translation[1]-offset[1];
    m->Element[2][3] = translation[2]-offset[2];

    vtkSmartPointer<vtkMatrix4x4> mInv = vtkMatrix4x4::New();
    vtkMatrix4x4::Invert(m, mInv);
    return mInv;
}

//画坐标轴
void createAxes(vtkSmartPointer<vtkRenderer> renderer, vtkSmartPointer<vtkAxesActor>& axes)
{
    //轴
    axes = vtkSmartPointer<vtkAxesActor>::New();
    axes->SetTotalLength(400.0, 400.0, 400.0);
    axes->SetShaftType(vtkAxesActor::LINE_SHAFT); //设置轴类型圆柱形状
    axes->SetCylinderRadius(0.01);
    axes->PickableOff();
    axes->SetConeRadius(0.01);
    axes->GetXAxisCaptionActor2D()->SetWidth(0.01);
    axes->GetYAxisCaptionActor2D()->SetWidth(0.01);
    axes->GetZAxisCaptionActor2D()->SetWidth(0.01);

    renderer->AddActor(axes);
}

int main(int argc, char* argv[])
{
    // Verify input arguments
    if ( argc != 2 )
    {
       std::cout << "Usage: " << argv[0]
       << " FolderName" << std::endl;
       return EXIT_FAILURE;
    }

    std::string folder = argv[1];
    //读取二维切片数据序列
    vtkSmartPointer< vtkDICOMImageReader >reader =
        vtkSmartPointer< vtkDICOMImageReader >::New();
    reader->SetDataByteOrderToLittleEndian();
    reader->SetDirectoryName(folder.c_str());//设置读取路径

    //reader->SetDataSpacing(1.0, 1.0, 1.0);//设置每个体素的大小
    reader->Update();


    //抽取等值面为骨头的信息
    //MC算法
    vtkSmartPointer< vtkMarchingCubes > boneExtractor =
        vtkSmartPointer< vtkMarchingCubes >::New();
    boneExtractor->SetInputConnection(reader->GetOutputPort());
    boneExtractor->SetValue(0, 400); //设置提取的等值信息
    boneExtractor->Update();


    //利用ContourFilter提取等值面
    /*vtkSmartPointer< vtkContourFilter > boneExtractor =
        vtkSmartPointer< vtkContourFilter >::New();
    boneExtractor->SetInputConnection(reader->GetOutputPort());
    boneExtractor->SetValue(0, 200); //设置提取的等值信息
    boneExtractor->Update();*/

    //DC算法   耗时长，模型有明显缝隙
    /*vtkSmartPointer< vtkRecursiveDividingCubes > boneExtractor =
        vtkSmartPointer< vtkRecursiveDividingCubes >::New();
    boneExtractor->SetInputConnection(reader->GetOutputPort());
    boneExtractor->SetValue(500);
    boneExtractor->SetDistance(1);
    boneExtractor->SetIncrement(2);
    boneExtractor->Update();*/

    //剔除旧的或废除的数据单元，提高绘制速度(可略去这一步)
    vtkSmartPointer< vtkStripper > boneStripper =
        vtkSmartPointer< vtkStripper >::New(); //三角带连接
    boneStripper->SetInputConnection(boneExtractor->GetOutputPort());
    boneStripper->Update();

    //平滑滤波
    vtkSmartPointer<vtkSmoothPolyDataFilter> pSmoothPolyDataFilter = vtkSmartPointer<vtkSmoothPolyDataFilter>::New();
    pSmoothPolyDataFilter->SetInputConnection(boneStripper->GetOutputPort());
    //pSmoothPolyDataFilter->SetNumberOfIterations(m_nNumberOfIterations);
    pSmoothPolyDataFilter->SetRelaxationFactor(0.05);

    vtkSmartPointer<vtkPolyDataNormals> pPolyDataNormals = vtkSmartPointer<vtkPolyDataNormals>::New();
    pPolyDataNormals->SetInputConnection(pSmoothPolyDataFilter->GetOutputPort());
    //pPolyDataNormals->SetFeatureAngle(m_nFeatureAngle);
    pPolyDataNormals->Update();


    //将模型输出到入STL文件
//    std::string filename = "/home/tiger/out_Smooth.stl";//设置输出文件路径

//    vtkSmartPointer<vtkSTLWriter> stlWriter =
//        vtkSmartPointer<vtkSTLWriter>::New();
//    stlWriter->SetFileName(filename.c_str());
//    stlWriter->SetInputConnection(pPolyDataNormals->GetOutputPort());
//    stlWriter->Write();


    //建立映射
    vtkSmartPointer< vtkPolyDataMapper > boneMapper =
        vtkSmartPointer< vtkPolyDataMapper >::New();
    boneMapper->SetInputData(pPolyDataNormals->GetOutput());
    boneMapper->ScalarVisibilityOff();
    //建立角色
    vtkSmartPointer< vtkActor > bone =
        vtkSmartPointer< vtkActor >::New();
    bone->SetMapper(boneMapper);

    bone->GetProperty()->SetDiffuseColor(1.0, 1.0, 1.0);
    bone->GetProperty()->SetSpecular(.3);
    bone->GetProperty()->SetSpecularPower(20);

    //定义绘制器
    vtkSmartPointer< vtkRenderer > aRenderer =
        vtkSmartPointer< vtkRenderer >::New();
    //定义绘制窗口
    vtkSmartPointer< vtkRenderWindow > renWin =
        vtkSmartPointer< vtkRenderWindow >::New();
    renWin->AddRenderer(aRenderer);
    //定义窗口交互器
    vtkSmartPointer< vtkRenderWindowInteractor > iren =
        vtkSmartPointer< vtkRenderWindowInteractor >::New();
    iren->SetRenderWindow(renWin);

    //创建一个camera
    vtkSmartPointer< vtkCamera > aCamera =
        vtkSmartPointer< vtkCamera >::New();
    aCamera->SetViewUp(0, 0, -1);
    aCamera->SetPosition(0, 1, 0);
    aCamera->SetFocalPoint(0, 0, 0);

    aRenderer->AddActor(bone);
    aRenderer->SetActiveCamera(aCamera);
    aRenderer->ResetCamera();
    aCamera->Dolly(1.5);
    aRenderer->SetBackground(0, 0, 0);
    aRenderer->ResetCameraClippingRange();

    vtkSmartPointer<vtkAxesActor> axes = vtkSmartPointer<vtkAxesActor>::New();
    createAxes(aRenderer, axes);


    // 加入二维精度测量工装上上对应的空间点，
    // {
    // 工装位置矩阵
    Eigen::Vector3d center2 = {0, 0, 0};
    Eigen::Vector3d translation2 = {-70, 270, 130};
    Eigen::Vector3d angInDegree2 = {180, 0, 0};
    vtkSmartPointer<vtkMatrix4x4> m_CtfrW = createMatrix_CtfrW(center2, translation2, angInDegree2);
    cout << "m_CtfrW = ";
    m_CtfrW->Print(cout);
    vector<Eigen::Vector3d> ironPoints1, ironPoints2;
    for(uint i=0; i< sPose1PtsIron_W.size(); i++) {
        ironPoints1.push_back(MeVtkMath::multiplyPoint(m_CtfrW, sPose1PtsIron_W[i]));
    }

    for(uint i=0; i< sPose2PtsIron_W.size(); i++) {
        ironPoints2.push_back(MeVtkMath::multiplyPoint(m_CtfrW, sPose2PtsIron_W[i]));
    }

    vtkSmartPointer<vtkActor> pointsActor1 = createActorWithPts(ironPoints1, colorRed);
    aRenderer->AddActor(pointsActor1);
    vtkSmartPointer<vtkActor> pointsActor2 = createActorWithPts(ironPoints2, colorGreen);
    aRenderer->AddActor(pointsActor2);

    // 4个特征点
    vector<Eigen::Vector3d> featurePoints;
    if(1){ // 加入3个固定小球点
        double spacing[3] = {0.488281, 0.488281, 0.625};
        featurePoints.push_back({204.59, 113.769,  136.25}); // 左耳朵
        featurePoints.push_back({56.6406, 103.027, 144.375}); // 右耳朵
        featurePoints.push_back({108.398, 215.8, 150.625}); // 鼻子（大）
        featurePoints.push_back({145.996, 215.82, 150.625}); // 鼻子（小）
    }
    vtkSmartPointer<vtkActor> pointsActor3 = createActorWithPts(featurePoints, colorGreen);
    aRenderer->AddActor(pointsActor3);

    // 红色标注第一个点和第三个点
    aRenderer->AddActor(createBallActor(featurePoints[0].data(), colorRed));
    aRenderer->AddActor(createBallActor(featurePoints[2].data(), colorRed));
    // }

    //将3D模型渲染到绘制窗口
    renWin->SetSize(800,800);
    iren->Initialize();
    iren->Start();

    return 0;
}
